/* ========================================================================
 * PROJECT: FTIR Blob Tracker
 * ========================================================================
 * This work is based on the original FTIR Blob Tracker developed by
 *   Woonhyuk Baek
 *   Woontack Woo
 *   U-VR Lab, GIST of Gwangju in Korea.
 * 
 *   http://uvr.gist.ac.kr/
 *
 * Copyright of the derived and new portions of this work
 *     (C) 2009 GIST U-VR Lab.
 *
 * This framework is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This framework is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this framework; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * For further information please contact 
 *   Woonhyuk Baek
 *   <windage@live.com>
 *   GIST U-VR Lab.
 *   Department of Information and Communication
 *   Gwangju Institute of Science and Technology
 *   1, Oryong-dong, Buk-gu, Gwangju
 *   South Korea
 * ========================================================================
 ** @author   Woonhyuk Baek
 * ======================================================================== */

#include "WarpFramework.h"
using namespace windage;

bool WarpFramework::UpdateControlPoint()
{
	for(int i=0; i<this->CONTROL_PRAMETER_COUNT; i++)
	{
		this->adjustmentParams[i] = this->homographyCalc->ConvertObjectToImage(imageParams[i]);
	}
	return true;
}

void WarpFramework::SetHomography(windage::Matrix3 h)
{
	this->homography = h;
	this->homographyCalc->SetHomography(h);	
	this->UpdateControlPoint();
	this->warpping->UpdateWarppingParameter(true, &this->homography);
}

int WarpFramework::GetNearestPoint(windage::Vector2 point, double* distance)
{
	int minIndex = -1;
	double minDistance = 1.0e10;
	for(int i=0; i<CONTROL_PRAMETER_COUNT; i++)
	{
		double tempDistance = point.getDistance(this->adjustmentParams[i]);
		if(minDistance > tempDistance)
		{
			minIndex = i;
			minDistance = tempDistance;
		}
	}

	if(distance) (*distance) = minDistance;
	return minIndex;
}

void WarpFramework::ChangeScaling(double scale)
{
	windage::Vector2 centerPoint = windage::Vector2(0.0, 0.0);
	for(int i=0; i<this->CONTROL_PRAMETER_COUNT; i++)
		centerPoint += this->adjustmentParams[i];
	centerPoint /= (double)this->CONTROL_PRAMETER_COUNT;
	
	for(int i=0; i<this->CONTROL_PRAMETER_COUNT; i++)
	{
		windage::Vector2 direction = this->adjustmentParams[i] - centerPoint;
		double distance = direction.getLength();
		direction /= distance;

		distance *= scale;
		direction *= distance;
		this->adjustmentParams[i] = centerPoint + direction;
	}
}

void WarpFramework::ChangeRotation(double radianAngle)
{
	windage::Vector2 centerPoint = windage::Vector2(0.0, 0.0);
	for(int i=0; i<this->CONTROL_PRAMETER_COUNT; i++)
		centerPoint += this->adjustmentParams[i];
	centerPoint /= (double)this->CONTROL_PRAMETER_COUNT;

	double cosAngle = cos(radianAngle);
	double sinAngle = sin(radianAngle);

	for(int i=0; i<this->CONTROL_PRAMETER_COUNT; i++)
	{
		windage::Vector2 direction = this->adjustmentParams[i] - centerPoint;
		double distance = direction.getLength();

		double dx = cosAngle*direction.x - sinAngle*direction.y;
		double dy = sinAngle*direction.x + cosAngle*direction.y;

		direction.x = dx;
		direction.y = dy;
		this->adjustmentParams[i] = centerPoint + direction;
	}
}

void WarpFramework::ChangeTranslation(windage::Vector2 deltaT)
{
	for(int i=0; i<this->CONTROL_PRAMETER_COUNT; i++)
	{
		this->adjustmentParams[i] += deltaT;
	}
}

bool WarpFramework::WarpImageFromHomography(IplImage* src, IplImage* dst)
{
	return this->warpping->Do(src, dst);
}

bool WarpFramework::UpdateHomographyFromChessboard(IplImage* src)
{
	std::vector<windage::Vector2> chessboardCorners;
	std::vector<windage::Vector2> chessboardOrigin;

	this->calibration->Undistortion(src, tempImage);
	this->calibrator->FindChessboardCorner(&chessboardCorners, tempImage);
	this->calibrator->GetChessboard(&chessboardOrigin);

	this->homographyCalc->ResetFlip();
	this->homographyCalc->AttatchImagePoints(&chessboardOrigin);
	this->homographyCalc->AttatchObjectPoints(&chessboardCorners);
	this->homographyCalc->Do();
	this->homographyCalc->SetFlip();

	this->UpdateControlPoint();
	for(int i=0; i<this->CONTROL_PRAMETER_COUNT; i++)
	{
		this->adjustmentParams[i].x += this->translationToCenterX;
		this->adjustmentParams[i].y += this->translationToCenterY;
	}

	this->UpdateHomographyFromPoints();

	return true;
}

bool WarpFramework::UpdateHomographyFromPoints()
{
	this->homographyCalc->SetFlip();
	this->homographyCalc->AttatchImagePoints(&this->adjustmentParams);
	this->homographyCalc->AttatchObjectPoints(&this->imageParams);
	this->homographyCalc->Do();

	this->homography = this->homographyCalc->GetHomography();
	this->warpping->UpdateWarppingParameter(true, &this->homography);
	return true;
}